#include "rendSkeleton.hpp"

#include <math.h>

namespace rend
{

bool
initBoneMatx(
	Material::EnvMat4v* bone_mat,
	Bone* bone,
	const unsigned bone_idx)
{
	assert(bone_mat);
	assert(bone);
	assert(bone_idx < bone_mat->count);

	if (bone[bone_idx].matx_valid)
		return false;

	bone[bone_idx].to_model = matx4(bone[bone_idx].orientation);

	vect3& row0 = vect3::castU(bone[bone_idx].to_model.dekast()[0]);
	vect3& row1 = vect3::castU(bone[bone_idx].to_model.dekast()[1]);
	vect3& row2 = vect3::castU(bone[bone_idx].to_model.dekast()[2]);

	row0.mul(bone[bone_idx].scale);
	row1.mul(bone[bone_idx].scale);
	row2.mul(bone[bone_idx].scale);

	bone[bone_idx].to_model.setColS(3, bone[bone_idx].position);

	const unsigned parent_idx(bone[bone_idx].parent_idx);

	if (parent_idx != 255)
	{
		if (!bone[parent_idx].matx_valid)
			initBoneMatx(bone_mat, bone, parent_idx);

		bone[bone_idx].to_model.mul(bone[parent_idx].to_model);
	}

	bone[bone_idx].to_local.invert(bone[bone_idx].to_model);

	bone_mat->array[bone_idx].identity();

	bone[bone_idx].matx_valid = true;

	return true;
}


bool
updateBoneMatx(
	Material::EnvMat4v* bone_mat,
	Bone* bone,
	const unsigned bone_idx)
{
	assert(bone_mat);
	assert(bone);
	assert(bone_idx < bone_mat->count);

	if (bone[bone_idx].matx_valid)
		return false;

	bone[bone_idx].to_model = matx4(bone[bone_idx].orientation);

	vect3& row0 = vect3::castU(bone[bone_idx].to_model.dekast()[0]);
	vect3& row1 = vect3::castU(bone[bone_idx].to_model.dekast()[1]);
	vect3& row2 = vect3::castU(bone[bone_idx].to_model.dekast()[2]);

	row0.mul(bone[bone_idx].scale);
	row1.mul(bone[bone_idx].scale);
	row2.mul(bone[bone_idx].scale);

	bone[bone_idx].to_model.setColS(3, bone[bone_idx].position);

	const unsigned parent_idx(bone[bone_idx].parent_idx);

	if (parent_idx != 255)
	{
		if (!bone[parent_idx].matx_valid)
			updateBoneMatx(bone_mat, bone, parent_idx);

		bone[bone_idx].to_model.mul(bone[parent_idx].to_model);
	}

	bone_mat->array[bone_idx].mul(bone[bone_idx].to_model, bone[bone_idx].to_local);
	bone_mat->array[bone_idx].transpose(); // ES does not allow trasposing uniforms on the fly

	bone[bone_idx].matx_valid = true;

	return true;
}


bool
invalidateBoneMatx(
	Bone* bone,
	const unsigned bone_idx,
	const unsigned num_invalid,
	const unsigned* invalid_idx)
{
	assert(bone);

	if (!bone[bone_idx].matx_valid)
		return true;

	for (unsigned i = 0; i < num_invalid; ++i)
		if (bone_idx == invalid_idx[i])
		{
			bone[bone_idx].matx_valid = false;

			return true;
		}

	const unsigned parent_idx(bone[bone_idx].parent_idx);

	if (parent_idx != 255 &&
		invalidateBoneMatx(bone, parent_idx, num_invalid, invalid_idx))
	{
		bone[bone_idx].matx_valid = false;
		return true;
	}

	return false;
}


void
animateSkeleton(
	Material::EnvMat4v* bone_mat,
	Bone* bone,
	const std::vector< Track >& skeletal_animation,
	const float anim_time)
{
	assert(bone_mat);
	assert(bone);

	if (0 == skeletal_animation.size())
		return;

	unsigned updates[Material::EnvMat4v::ARRAY_CAPACITY];
	unsigned n_updates = 0;

	for (std::vector< Track >::const_iterator it = skeletal_animation.begin(); it != skeletal_animation.end(); ++it)
	{
		assert(it->bone_idx < bone_mat->count);

		std::vector< Track::BonePositionKey >::const_iterator jt0 = it->position_key.begin() + it->position_last_key_idx;
		std::vector< Track::BoneOrientationKey >::const_iterator jt1 = it->orientation_key.begin() + it->orientation_last_key_idx;
		std::vector< Track::BoneScaleKey >::const_iterator jt2 = it->scale_key.begin() + it->scale_last_key_idx;

		for (std::vector< Track::BonePositionKey >::const_iterator kt = it->position_key.end();
			jt0 != it->position_key.end(); ++jt0)	
		{
			if (jt0->time < anim_time)
			{
				kt = jt0;
				it->position_last_key_idx = jt0 - it->position_key.begin();

				continue;
			}

			if (jt0->time > anim_time)
			{
				if (kt == it->position_key.end())
				{
					jt0 = kt;
					break;
				}

				const float w1 = (anim_time - kt->time) / (jt0->time - kt->time);
				const float w0 = 1.f - w1;

				bone[it->bone_idx].position.wsum(kt->value, jt0->value, w0, w1);
			}
			else
				bone[it->bone_idx].position = jt0->value;

			break;
		}

		for (std::vector< Track::BoneOrientationKey >::const_iterator kt = it->orientation_key.end();
			jt1 != it->orientation_key.end(); ++jt1)	
		{
			if (jt1->time < anim_time)
			{
				kt = jt1;
				it->orientation_last_key_idx = jt1 - it->orientation_key.begin();

				continue;
			}

			if (jt1->time > anim_time)
			{
				if (kt == it->orientation_key.end())
				{
					jt1 = kt;
					break;
				}

				const float w1 = (anim_time - kt->time) / (jt1->time - kt->time);
				const float w0 = 1.f - w1;

				bone[it->bone_idx].orientation.wsum(kt->value, jt1->value, w0, w1);
				bone[it->bone_idx].orientation.normalise();
			}
			else
				bone[it->bone_idx].orientation = jt1->value;

			break;
		}

		for (std::vector< Track::BoneScaleKey >::const_iterator kt = it->scale_key.end();
			jt2 != it->scale_key.end(); ++jt2)	
		{
			if (jt2->time < anim_time)
			{
				kt = jt2;
				it->scale_last_key_idx = jt2 - it->scale_key.begin();

				continue;
			}

			if (jt2->time > anim_time)
			{
				if (kt == it->scale_key.end())
				{
					jt2 = kt;
					break;
				}

				const float w1 = (anim_time - kt->time) / (jt2->time - kt->time);
				const float w0 = 1.f - w1;

				bone[it->bone_idx].scale.wsum(kt->value, jt2->value, w0, w1);
			}
			else
				bone[it->bone_idx].scale = jt2->value;

			break;
		}

		if (jt0 != it->position_key.end() ||
			jt1 != it->orientation_key.end() ||
			jt2 != it->scale_key.end())
		{
			updates[n_updates++] = it->bone_idx;
		}
	}

	if (n_updates)
	{
		for (unsigned i = 0; i < bone_mat->count; ++i)
			invalidateBoneMatx(bone, i, n_updates, updates);

		for (unsigned i = 0; i < bone_mat->count; ++i)
			updateBoneMatx(bone_mat, bone, i);
	}
}


void
resetSkeletonAnimProgress(
	const std::vector< Track >& skeletal_animation)
{
	for (std::vector< Track >::const_iterator it = skeletal_animation.begin(); it != skeletal_animation.end(); ++it)
	{
		it->position_last_key_idx = 0;
		it->orientation_last_key_idx = 0;
		it->scale_last_key_idx = 0;
	}
}


bool
loadSkeletonAnimationAge(
	Material::EnvMat4v* bone_mat,
	Bone* bone,
	std::vector< Track >& skeletal_animation,
	FILE* f)
{
	assert(bone_mat);
	assert(bone);
	assert(f);

	uint32 magic;
	uint32 version;

	fread(&magic, sizeof(magic), 1, f);
	fread(&version, sizeof(version), 1, f);

	fprintf(stdout, "skeleton magic, version: %.4s, %u\n", (char*) &magic, version);

	static const uint32 sMagic('lekS');
	static const uint32 sVersion(100);

	assert(magic == sMagic);
	assert(version == sVersion);

	if (magic != sMagic || version != sVersion)
		return false;

	uint16 n_bones;
	fread(&n_bones, sizeof(n_bones), 1, f);

	assert(n_bones <= Material::EnvMat4v::ARRAY_CAPACITY);

	for (uint16 i = 0; i < n_bones; ++i)
	{
		fread(&bone[i].position, sizeof(bone[0].position), 1, f);

		vect4 ori_swizz;
		fread(&ori_swizz, sizeof(ori_swizz), 1, f);
		ori_swizz.normalise();

		bone[i].orientation.set(0, ori_swizz[1]);
		bone[i].orientation.set(1, ori_swizz[2]);
		bone[i].orientation.set(2, ori_swizz[3]);
		bone[i].orientation.set(3, ori_swizz[0]);

		fread(&bone[i].scale, sizeof(bone[0].scale), 1, f);
		fread(&bone[i].parent_idx, sizeof(bone[0].parent_idx), 1, f);

		uint16 name_len;
		fread(&name_len, sizeof(name_len), 1, f);

		char name[1024];
		fread(name, sizeof(name[0]), name_len, f);
		name[name_len] = '\0';

		bone[i].name = name;

		fprintf(stdout,
			"skeleton bone %u: %s\n"
			"\tpos: %f, %f, %f\n"
			"\tori: %f, %f, %f, %f\n"
			"\tsca: %f, %f, %f\n"
			"\tpar: %u\n",
			i, name,
			bone[i].position[0],
			bone[i].position[1],
			bone[i].position[2],
			bone[i].orientation[0],
			bone[i].orientation[1],
			bone[i].orientation[2],
			bone[i].orientation[3],
			bone[i].scale[0],
			bone[i].scale[1],
			bone[i].scale[2],
			unsigned(bone[i].parent_idx));
	}

	uint16 n_anims;
	fread(&n_anims, sizeof(n_anims), 1, f);

	for (uint16 i = 0; i < n_anims; ++i)
	{
		uint16 name_len;
		fread(&name_len, sizeof(name_len), 1, f);

		char name[1024];
		fread(name, sizeof(name[0]), name_len, f);
		name[name_len] = '\0';

		uint32 dummy[4];
		fread(dummy, sizeof(dummy[0]), 2, f);

		uint16 n_tracks;
		fread(&n_tracks, sizeof(n_tracks), 1, f);

		fprintf(stdout, "animation, tracks: %s, %u\n", name, unsigned(n_tracks));

		for (uint16 i = 0; i < n_tracks; ++i)
		{
			uint8 bone_idx_and_stuff[2];
			fread(bone_idx_and_stuff, sizeof(bone_idx_and_stuff), 1, f);

			assert(!bone_idx_and_stuff[1]);

			fprintf(stdout, "track, bone: %u, %u\n", unsigned(i), unsigned(bone_idx_and_stuff[0]));

			if (bone_idx_and_stuff[0] == 255)
				bone_idx_and_stuff[0] = 0;

			Track& track = *skeletal_animation.insert(skeletal_animation.end(), Track());

			track.bone_idx = bone_idx_and_stuff[0];

			uint32 n_pos_keys;
			fread(&n_pos_keys, sizeof(n_pos_keys), 1, f);

			fprintf(stdout, "\tposition keys: %u\n", unsigned(n_pos_keys));

			for (uint32 i = 0; i < n_pos_keys; ++i)
			{
				Track::BonePositionKey& pos = *track.position_key.insert(track.position_key.end(), Track::BonePositionKey());

				fread(&pos.time, sizeof(pos.time), 1, f);
				fread(&pos.value, sizeof(pos.value), 1, f);
			}

			uint32 n_ori_keys;
			fread(&n_ori_keys, sizeof(n_ori_keys), 1, f);

			fprintf(stdout, "\torientation keys: %u\n", unsigned(n_ori_keys));

			for (uint32 i = 0; i < n_ori_keys; ++i)
			{
				Track::BoneOrientationKey& ori = *track.orientation_key.insert(track.orientation_key.end(), Track::BoneOrientationKey());
				vect4 ori_swizz;

				fread(&ori.time, sizeof(ori.time), 1, f);
				fread(&ori_swizz, sizeof(ori_swizz), 1, f);

				ori.value.set(0, ori_swizz[1]);
				ori.value.set(1, ori_swizz[2]);
				ori.value.set(2, ori_swizz[3]);
				ori.value.set(3, ori_swizz[0]);
			}

			uint32 n_sca_keys;
			fread(&n_sca_keys, sizeof(n_sca_keys), 1, f);

			fprintf(stdout, "\tscale keys: %u\n", unsigned(n_sca_keys));

			for (uint32 i = 0; i < n_sca_keys; ++i)
			{
				Track::BoneScaleKey& sca = *track.scale_key.insert(track.scale_key.end(), Track::BoneScaleKey());

				fread(&sca.time, sizeof(sca.time), 1, f);
				fread(&sca.value, sizeof(sca.value), 1, f);
			}

			track.position_last_key_idx = 0;
			track.orientation_last_key_idx = 0;
			track.scale_last_key_idx = 0;
		}

		break; // don't read any further
	}

	bone_mat->count = n_bones;

	for (uint16 i = 0; i < n_bones; ++i)
		initBoneMatx(bone_mat, bone, unsigned(i));

	return true;
}

} // namespace rend
